home *** CD-ROM | disk | FTP | other *** search
/ Language/OS - Multiplatform Resource Library / LANGUAGE OS.iso / cpp_libs / vector.lha / vector / adjoint.c next >
C/C++ Source or Header  |  1991-11-23  |  5KB  |  195 lines
  1. /*
  2. Matrix Inversion
  3. by Richard Carling
  4. from "Graphics Gems", Academic Press, 1990
  5. */
  6. #include <stdio.h>
  7.  
  8. #define SMALL_NUMBER    1.e-8
  9.  
  10. typedef struct Matrix4Struct {    /* 4-by-4 matrix */
  11.     float element[4][4];
  12.     } Matrix4;
  13.  
  14. float inverse(/* Matrix4 *, Matrix4 * */);
  15. float adjoint(/* Matrix4 *, Matrix4 * */);
  16.  
  17. /*
  18.  *   inverse( original_matrix, inverse_matrix )
  19.  *
  20.  *    calculate the inverse of a 4x4 matrix
  21.  *
  22.  *     -1
  23.  *     A  = ___1__ adjoint A
  24.  *       det A
  25.  */
  26.  
  27. float inverse( in, out ) Matrix4 *in, *out;
  28. {
  29.     int i, j;
  30.     float det, det4x4();
  31.  
  32.     /* calculate the adjoint matrix & its determinant.
  33.      * if the deteminant is zero, then the inverse matrix is not unique.
  34.      */
  35.  
  36.     det = adjoint( in, out );
  37.  
  38.     if ( fabs( det ) < SMALL_NUMBER) {
  39.     fprintf(stderr, "Non-singular matrix, no inverse!\n");
  40.     exit(1);
  41.     }
  42.  
  43.     /* scale the adjoint matrix to get the inverse */
  44.  
  45.     for (i=0; i<4; i++)
  46.     for(j=0; j<4; j++)
  47.         out->element[i][j] = out->element[i][j] / det;
  48.  
  49.     return det;
  50. }
  51.  
  52.  
  53. /*
  54.  *   adjoint( original_matrix, inverse_matrix )
  55.  *
  56.  *     calculate the adjoint of a 4x4 matrix
  57.  *
  58.  *    Let  a     denote the minor determinant of matrix A obtained by
  59.  *          ij
  60.  *
  61.  *    deleting the ith row and jth column from A.
  62.  *
  63.  *              i+j
  64.  *     Let  b    = (-1)      a
  65.  *         ij           ji
  66.  *
  67.  *    The matrix B = (b  ) is the adjoint of A
  68.  *               ij
  69.  */
  70.  
  71. float adjoint( in, out ) Matrix4 *in; Matrix4 *out;
  72. {
  73.     float a1, a2, a3, a4, b1, b2, b3, b4;
  74.     float c1, c2, c3, c4, d1, d2, d3, d4;
  75.     float det, det3x3();
  76.  
  77.     /* assign to individual variable names to aid  */
  78.     /* selecting correct values  */
  79.  
  80.     a1 = in->element[0][0]; b1 = in->element[0][1];
  81.     c1 = in->element[0][2]; d1 = in->element[0][3];
  82.  
  83.     a2 = in->element[1][0]; b2 = in->element[1][1];
  84.     c2 = in->element[1][2]; d2 = in->element[1][3];
  85.  
  86.     a3 = in->element[2][0]; b3 = in->element[2][1];
  87.     c3 = in->element[2][2]; d3 = in->element[2][3];
  88.  
  89.     a4 = in->element[3][0]; b4 = in->element[3][1];
  90.     c4 = in->element[3][2]; d4 = in->element[3][3];
  91.  
  92.  
  93.     /* row column labeling reversed since we transpose rows & columns */
  94.  
  95.     out->element[0][0]    =   det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
  96.     out->element[1][0]    = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
  97.     out->element[2][0]    =   det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
  98.     out->element[3][0]    = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
  99.  
  100.     out->element[0][1]    = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
  101.     out->element[1][1]    =   det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
  102.     out->element[2][1]    = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
  103.     out->element[3][1]    =   det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
  104.  
  105.     out->element[0][2]    =   det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
  106.     out->element[1][2]    = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
  107.     out->element[2][2]    =   det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
  108.     out->element[3][2]    = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
  109.  
  110.     out->element[0][3]    = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
  111.     out->element[1][3]    =   det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
  112.     out->element[2][3]    = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
  113.     out->element[3][3]    =   det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
  114.  
  115.     det = a1 * out->element[0][0]
  116.     + b1 * out->element[1][0]
  117.     + c1 * out->element[2][0]
  118.     + d1 * out->element[3][0];
  119.  
  120.     return det;
  121. }
  122. /*
  123.  * float = det4x4( matrix )
  124.  *
  125.  * calculate the determinent of a 4x4 matrix.
  126.  */
  127. float det4x4( m ) Matrix4 *m;
  128. {
  129.     float ans;
  130.     float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3,               d4;
  131.     float det3x3();
  132.  
  133.     /* assign to individual variable names to aid selecting */
  134.     /*  correct elements */
  135.  
  136.     a1 = m->element[0][0]; b1 = m->element[0][1];
  137.     c1 = m->element[0][2]; d1 = m->element[0][3];
  138.  
  139.     a2 = m->element[1][0]; b2 = m->element[1][1];
  140.     c2 = m->element[1][2]; d2 = m->element[1][3];
  141.  
  142.     a3 = m->element[2][0]; b3 = m->element[2][1];
  143.     c3 = m->element[2][2]; d3 = m->element[2][3];
  144.  
  145.     a4 = m->element[3][0]; b4 = m->element[3][1];
  146.     c4 = m->element[3][2]; d4 = m->element[3][3];
  147.  
  148.     ans = a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
  149.     - b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
  150.     + c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
  151.     - d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
  152.     return ans;
  153. }
  154.  
  155.  
  156.  
  157. /*
  158.  * float = det3x3(  a1, a2, a3, b1, b2, b3, c1, c2, c3 )
  159.  *
  160.  * calculate the determinent of a 3x3 matrix
  161.  * in the form
  162.  *
  163.  *     | a1,  b1,  c1 |
  164.  *     | a2,  b2,  c2 |
  165.  *     | a3,  b3,  c3 |
  166.  */
  167.  
  168. float det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3 )
  169. float a1, a2, a3, b1, b2, b3, c1, c2, c3;
  170. {
  171.     float ans;
  172.     float det2x2();
  173.  
  174.     ans = a1 * det2x2( b2, b3, c2, c3 )
  175.     - b1 * det2x2( a2, a3, c2, c3 )
  176.     + c1 * det2x2( a2, a3, b2, b3 );
  177.     return ans;
  178. }
  179.  
  180. /*
  181.  * float = det2x2( float a, float b, float c, float d )
  182.  *
  183.  * calculate the determinent of a 2x2 matrix.
  184.  */
  185.  
  186. float det2x2( a, b, c, d)
  187. float a, b, c, d;
  188. {
  189.     float ans;
  190.     ans = a * d - b * c;
  191.     return ans;
  192. }
  193.  
  194.  
  195.